﻿#include "spline_interpolation.h"
#include "spline.h"
#include "raster_surface.h"

#include <pcl/filters/extract_indices.h>
#include <pcl/console/print.h>
#include <pcl/common/io.h>


#include <vector>
#include <memory>

using namespace std;
namespace oil
{
SplineInterpolation::SplineInterpolation(void)
{
}


SplineInterpolation::~SplineInterpolation(void)
{
}

void SplineInterpolation::compute()
{
	init();
#pragma omp parallel for
	for (int r = 0; r < rows_; ++r)
	{
		for (int c = 0; c < cols_; ++c)
		{
			pcl::PointXY center_point;
			center_point.x = min_.x() + c * window_size_;
			center_point.y = min_.y() + r * window_size_;

			std::vector<float> knn_distance;
			IndicesPtr knn_index(new vector<int>);
			searcher_.nearestKSearch(center_point, search_size_, *knn_index,
				knn_distance);

			PointCloudPtr knn_cloud(new PointCloud);
			pcl::ExtractIndices<PointXYZ> extract;
			extract.setInputCloud(input_);
			extract.setIndices(knn_index);
			extract.setNegative(false);
			extract.filter(*knn_cloud);

			Spline spline(knn_cloud, lambda_);

			pcl::PointXYZ center_point3d(center_point.x, center_point.y, 0.0f);
			double avg = 0.0;
			for_each(begin(*knn_cloud), end(*knn_cloud), [&](const PointXYZ& p){
				avg += p.z;
			});
			avg /= search_size_;

			spline.interpolate(center_point3d);
			(*height_surface_)(r, c) = center_point3d.z;
			(*bending_energy_)(r, c) = spline.getBending();
			(*bending_mask_)(r, c) = center_point3d.z > avg ? 1.0f : 0.0f;
		}
	}
}

void SplineInterpolation::init()
{
	if (!input_)
	{
		PCL_ERROR("[SplineInterpolation::compute] initCompute failed!\n");
	}
	height_surface_ = shared_ptr<RasterSurface>(new RasterSurface(min_,
		max_, window_size_, window_size_));
	bending_energy_ = shared_ptr<RasterSurface>(new RasterSurface(min_,
		max_, window_size_, window_size_));
	bending_mask_ = shared_ptr<RasterSurface>(new RasterSurface(min_,
		max_, window_size_, window_size_));

	rows_ = height_surface_->rows();
	cols_ = height_surface_->cols();

	max_.x() = min_.x() + cols_ * window_size_;
	max_.y() = min_.y() + rows_ * window_size_;

	pcl::PointCloud<pcl::PointXY>::Ptr xy_search(
		new pcl::PointCloud<pcl::PointXY>());

	pcl::copyPointCloud(*input_, *xy_search);
	searcher_.setInputCloud(xy_search);
}

void SplineInterpolation::update(const vector<int>& changed_grids)
{
	if (!input_)
	{
		PCL_ERROR("[SplineInterpolation::compute] initCompute failed!\n");
	}
	pcl::PointCloud<pcl::PointXY>::Ptr xy_search(
		new pcl::PointCloud<pcl::PointXY>());

	pcl::copyPointCloud(*input_, *xy_search);
	searcher_.setInputCloud(xy_search);
#pragma omp parallel for
	for (int i = 0; i < changed_grids.size(); ++i)
	{
		int id = changed_grids[i];
		int r = id / cols_;
		int c = id - r * cols_;

		pcl::PointXY center_point;
		center_point.x = min_.x() + c * window_size_;
		center_point.y = min_.y() + r * window_size_;

		std::vector<float> knn_distance;
		IndicesPtr knn_index(new vector<int>);
		searcher_.nearestKSearch(center_point, search_size_, *knn_index,
			knn_distance);

		PointCloudPtr knn_cloud(new PointCloud);
		pcl::ExtractIndices<PointXYZ> extract;
		extract.setInputCloud(input_);
		extract.setIndices(knn_index);
		extract.setNegative(false);
		extract.filter(*knn_cloud);

		Spline spline(knn_cloud, lambda_);

		pcl::PointXYZ center_point3d(center_point.x, center_point.y, 0.0f);
		double avg = 0.0;
		for_each(begin(*knn_cloud), end(*knn_cloud), [&](const PointXYZ& p){
			avg += p.z;
		});
		avg /= search_size_;

		spline.interpolate(center_point3d);
		(*height_surface_)(r, c) = center_point3d.z;
		(*bending_energy_)(r, c) = spline.getBending();
		(*bending_mask_)(r, c) = center_point3d.z > avg ? 1.0f : 0.0f;
	}
}

}//end oil
